//
// Created by qzj on 2021/4/8.
//

#include "GroundPlaneFit.h"

using Eigen::MatrixXf;
using Eigen::JacobiSVD;
using Eigen::VectorXf;

GroundPlaneFit::GroundPlaneFit(){

//    传感器先验高度
    sensor_height_ = 0.408;

//    整个过程循环迭代次数
    num_iter_ = 3;

//    进行一个按照高度的排序，选出N个高度最低的点，并求取高度平均值，作为后续初始种子的评价
    num_lpr_ = 20;

//    被选为种子点的与高度平均值的差的阈值
    th_seeds_ = 0.5;

//    属于地平面的阈值FF
    th_dist_ = 0.05;

    g_seeds_pc.reset(new pcl::PointCloud<PointType>());
    g_not_ground_pc.reset(new pcl::PointCloud<PointType>());

}

/*
    @brief The function to estimate plane model. The
    model parameter `normal_` and `d_`, and `th_dist_d_`
    is set here.
    The main step is performed SVD(UAV) on covariance matrix.
    Taking the sigular vector in U matrix according to the smallest
    sigular value in A, as the `normal_`. `d_` is then calculated
    according to mean ground points.

    @param g_ground_pc:global ground pointcloud ptr.

*/
void GroundPlaneFit::estimate_plane_(const pcl::PointCloud<PointType>::Ptr &g_ground_pc){
    // Create covarian matrix in single pass.
    // TODO: compare the efficiency.
    Eigen::Matrix3f cov;
    Eigen::Vector4f pc_mean;

//    ROS_INFO("g_ground_pc %d",g_ground_pc->size());
//    for(int i=0;i<g_ground_pc->size();i++)
//        ROS_INFO("point %d, %f, %f, %f",i, g_ground_pc->points[i].x, g_ground_pc->points[i].y, g_ground_pc->points[i].z);

    pcl::computeMeanAndCovarianceMatrix(*g_ground_pc, cov, pc_mean);
//    std::cout<<"cov.(): "<<cov<<std::endl;
//    std::cout<<"pc_mean.(): "<<pc_mean.transpose()<<std::endl;
    // Singular Value Decomposition: SVD
    JacobiSVD<MatrixXf> svd(cov,Eigen::DecompositionOptions::ComputeFullU);
    // use the least singular vector as normal
    normal_ = (svd.matrixU().col(2));
    // mean ground seeds value
    Eigen::Vector3f seeds_mean = pc_mean.head<3>();


//    std::cout<<"svd.matrixU(): "<<svd.matrixU()<<std::endl;

//    std::cout<<"normal_: "<<normal_.transpose()<<" seeds_mean: "<<seeds_mean.transpose()<<std::endl;

    // according to normal.T*[x,y,z] = -d
    d_ = -(normal_.transpose()*seeds_mean)(0,0);
    // set distance threhold to `th_dist - d`
    th_dist_d_ = th_dist_ - d_;

//    std::cout<<"th_dist_d_: "<<th_dist_d_<<std::endl;

    // return the equation parameters
}


/*
    @brief Extract initial seeds of the given pointcloud sorted segment.
    This function filter ground seeds points accoring to heigt.
    This function will set the `g_ground_pc` to `g_seed_pc`.
    @param p_sorted: sorted pointcloud

    @param ::num_lpr_: num of LPR points    论文Fast Segmentation of 3D Point Clouds: A Paradigm on LiDAR Data for Autonomous Vehicle Applications中的内容
    @param ::th_seeds_: threshold distance of seeds
    @param ::

*/
void GroundPlaneFit::extract_initial_seeds_(const pcl::PointCloud<PointType>& p_sorted){
    // LPR is the mean of low point representative
    double sum = 0;
    int cnt = 0;
    // Calculate the mean height value.
    for(int i=0;i<p_sorted.points.size() && cnt<num_lpr_;i++){
        sum += p_sorted.points[i].z;
        cnt++;
    }
    double lpr_height = cnt!=0?sum/cnt:0;// in case divide by 0
    g_seeds_pc->clear();
    // iterate pointcloud, filter those height is less than lpr.height+th_seeds_
    for(int i=0;i<p_sorted.points.size();i++){
        if(p_sorted.points[i].z < lpr_height + th_seeds_){
            g_seeds_pc->points.push_back(p_sorted.points[i]);
        }
    }
    // return seeds points
}

void GroundPlaneFit::setInputCloud(const pcl::PointCloud<PointType>::Ptr& in_cloud){
    laserCloudIn.clear();
    laserCloudIn_org.clear();
    pcl::copyPointCloud(*in_cloud, laserCloudIn);
    // Remove Nan points
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
    pcl::copyPointCloud(laserCloudIn, laserCloudIn_org);


//    ROS_INFO("laserCloudIn_org width %d, height %d, size %d",laserCloudIn_org.width,laserCloudIn_org.height,laserCloudIn_org.size());
//    ROS_INFO("width %d, height %d, size %d",groundCloud->width,groundCloud->height,groundCloud->size());
}

/*
    @brief Velodyne pointcloud callback function. The main GPF pipeline is here.
    PointCloud SensorMsg -> Pointcloud -> z-value sorted Pointcloud
    ->error points removal -> extract ground seeds -> ground plane fit mainloop
*/
void GroundPlaneFit::filterGround(pcl::PointCloud<PointType>::Ptr& g_ground_pc,
                                  Eigen::MatrixXf &normal_out,float &th_dist_d_out){

    g_ground_pc->clear();
    g_not_ground_pc->clear();
    g_seeds_pc->clear();

    // 2.Sort on Z-axis value.
    sort(laserCloudIn.points.begin(),laserCloudIn.end(),point_cmp);//设置比较方式函数为point_cmp

    // 3.Error point removal
    // As there are some error mirror reflection under the ground,
    // here regardless point under 2* sensor_height
    // Sort point according to height, here uses z-axis in default
    pcl::PointCloud<PointType>::iterator it = laserCloudIn.points.begin();
    for(int i=0;i<laserCloudIn.points.size();i++){
        if(laserCloudIn.points[i].z < -1.5*sensor_height_){
            it++;
        }else{
            break;
        }
    }
    laserCloudIn.points.erase(laserCloudIn.points.begin(),it);//删去地面之下的点

    // 4. Extract init ground seeds.
    extract_initial_seeds_(laserCloudIn);
    g_ground_pc = g_seeds_pc;

//    PointType min;	//xyz的最小值
//    PointType max;	//xyz的最大值
//    pcl::getMinMax3D(*g_ground_pc,min,max);	//获取所有点中的坐标最值
//    double nx = (max.x - min.x);
//    double ny = (max.y - min.y);
//    double nz = (max.z - min.z);
//    ROS_INFO("nx %f, ny %f, nz %f",nx,ny,nz);
//    ROS_INFO("out g_ground_pc %d",g_ground_pc->size());
//    ROS_INFO("out g_seeds_pc %d",g_seeds_pc->size());

    // 5. Ground plane fitter mainloop
    for(int i=0;i<num_iter_;i++){
        estimate_plane_(g_ground_pc);
        g_ground_pc->clear();
        g_not_ground_pc->clear();

        //pointcloud to matrix
        MatrixXf points(laserCloudIn_org.points.size(),3);
        int j =0;
        for(auto p:laserCloudIn_org.points){
            points.row(j++)<<p.x,p.y,p.z;
        }
        // ground plane model
        VectorXf result = points*normal_;
        // threshold filter
        for(int r=0;r<result.rows();r++){
            if(result[r]<th_dist_d_){
                g_ground_pc->points.push_back(laserCloudIn_org[r]);
            }else{
                g_not_ground_pc->points.push_back(laserCloudIn_org[r]);
            }
        }
    }

    g_ground_pc->width = g_ground_pc->size();
    g_ground_pc->height = 1;
    g_not_ground_pc->width = g_not_ground_pc->size();
    g_not_ground_pc->height = 1;

    normal_out = normal_;
    th_dist_d_out = th_dist_d_;
}

void GroundPlaneFit::filterGround(pcl::PointCloud<PointType>::Ptr& g_ground_pc){

    g_ground_pc->clear();
    g_not_ground_pc->clear();
    g_seeds_pc->clear();

    // 2.Sort on Z-axis value.
    sort(laserCloudIn.points.begin(),laserCloudIn.end(),point_cmp);//设置比较方式函数为point_cmp

    // 3.Error point removal
    // As there are some error mirror reflection under the ground,
    // here regardless point under 2* sensor_height
    // Sort point according to height, here uses z-axis in default
    pcl::PointCloud<PointType>::iterator it = laserCloudIn.points.begin();
    for(int i=0;i<laserCloudIn.points.size();i++){
        if(laserCloudIn.points[i].z < -1.5*sensor_height_){
            it++;
        }else{
            break;
        }
    }
    laserCloudIn.points.erase(laserCloudIn.points.begin(),it);//删去地面之下的点

    // 4. Extract init ground seeds.
    extract_initial_seeds_(laserCloudIn);
    g_ground_pc = g_seeds_pc;

    // 5. Ground plane fitter mainloop
    for(int i=0;i<num_iter_;i++){
        estimate_plane_(g_ground_pc);
        g_ground_pc->clear();
        g_not_ground_pc->clear();

        //pointcloud to matrix
        MatrixXf points(laserCloudIn_org.points.size(),3);
        int j =0;
        for(auto p:laserCloudIn_org.points){
            points.row(j++)<<p.x,p.y,p.z;
        }
        // ground plane model
        VectorXf result = points*normal_;
        // threshold filter
        for(int r=0;r<result.rows();r++){
            if(result[r]<th_dist_d_){
                g_ground_pc->points.push_back(laserCloudIn_org[r]);
            }else{
                g_not_ground_pc->points.push_back(laserCloudIn_org[r]);
            }
        }
    }

    g_ground_pc->width = g_ground_pc->size();
    g_ground_pc->height = 1;
    g_not_ground_pc->width = g_not_ground_pc->size();
    g_not_ground_pc->height = 1;

}
void GroundPlaneFit::filterGround(pcl::PointCloud<pcl::PointXYZI>::Ptr& g_ground_pc,
                  pcl::PointCloud<pcl::PointXYZI>::Ptr& noground_pc){
    g_ground_pc->clear();
    noground_pc->clear();
    g_seeds_pc->clear();

    // 2.Sort on Z-axis value.
    sort(laserCloudIn.points.begin(),laserCloudIn.end(),point_cmp);//设置比较方式函数为point_cmp

    // 3.Error point removal
    // As there are some error mirror reflection under the ground,
    // here regardless point under 2* sensor_height
    // Sort point according to height, here uses z-axis in default
    pcl::PointCloud<PointType>::iterator it = laserCloudIn.points.begin();
    for(int i=0;i<laserCloudIn.points.size();i++){
        if(laserCloudIn.points[i].z < -1.5*sensor_height_){
            it++;
        }else{
            break;
        }
    }
    laserCloudIn.points.erase(laserCloudIn.points.begin(),it);//删去地面之下的点

    // 4. Extract init ground seeds.
    extract_initial_seeds_(laserCloudIn);
    g_ground_pc = g_seeds_pc;

    // 5. Ground plane fitter mainloop
    for(int i=0;i<num_iter_;i++){
        estimate_plane_(g_ground_pc);
        g_ground_pc->clear();
        g_not_ground_pc->clear();

        //pointcloud to matrix
        MatrixXf points(laserCloudIn_org.points.size(),3);
        int j =0;
        for(auto p:laserCloudIn_org.points){
            points.row(j++)<<p.x,p.y,p.z;
        }
        // ground plane model
        VectorXf result = points*normal_;
        // threshold filter
        for(int r=0;r<result.rows();r++){
            if(result[r]<th_dist_d_){
                g_ground_pc->points.push_back(laserCloudIn_org[r]);
            }else{
                noground_pc->points.push_back(laserCloudIn_org.points[r]);
            }
        }
    }

    g_ground_pc->width = g_ground_pc->size();
    g_ground_pc->height = 1;
    noground_pc->width = noground_pc->size();
    noground_pc->height = 1;
}